#include "MBEnergyMinimizer.h"
#include "MBCameraViewWithShiftVector.h"
#include "MBDirectoryMethods.h"
#include "MBDistanceMap.h"
#include "MBImageManager.h"
#include "MBTimer.h"

// GCO v3.0
#include "GCoptimization.h"
#include "graph.h"

#include <assert.h>
#include <iostream>
#include <limits>
#include <map>
#include <set>
#include <string>

using namespace MB;

using std::cout;
using std::endl;
using std::make_pair;
using std::map;
using std::max;
using std::min;
using std::multimap;
using std::numeric_limits;
using std::pair;
using std::set;
using std::string;
using std::vector;

const MBuint U = 0u;
const MBuint V = 1u;

const MBuint X = 0u;
const MBuint Y = 1u;

///////////////////////////////////////////////////////////////////////////////
// Local struct and class definitions BEGIN

/**
  *	An MBGraphCutNeighboringNodeIndices object capsules the indices of two
  * neighboring nodes in a graph
  */
class MBGraphCutNeighboringNodeIndices
{
public:
	/**
	  *	Constructs a new MBGraphCutNeighboringNodeIndices object with the given
	  * indices
	  *
	  * @param first index of the first node
	  * @param second index of the second node
	  */
	MBGraphCutNeighboringNodeIndices( const MBuint &first, const MBuint &second ) :
		mFirst( first ),
		mSecond( second ) {};

	/**
	  *	Returns true if this MBGraphCutNeighboringNodeIndices object is smaller
	  * than the other one, false otherwise. The indices are first sorted, then
	  * the first indices are compared, then the second.
	  *
	  * @param compareNode MBGraphCutNeighboringNodeIndices object that is compared to this one
	  * @return true if this MBGraphCutNeighboringNodeIndices object is smaller than the other one, false otherwise
	  */
	bool operator<( const MBGraphCutNeighboringNodeIndices &compareNode ) const
	{
		MBuint first[] = { mFirst, mSecond };
		MBuint second[] = { compareNode.mFirst, compareNode.mSecond };

		MBuint helper;

		// Sort
		if (first[0] > first[1])
		{
			helper = first[0];
			first[0] = first[1];
			first[1] = helper;
		}

		if (second[0] > second[1])
		{
			helper = second[0];
			second[0] = second[1];
			second[1] = helper;
		}

		// Compare
		if (first[0] < second[0])
		{
			return true;
		}
		else if (first[0] > second[0])
		{
			return false;
		}
		else
		{
			if (first[1] < second[1])
			{
				return true;
			}

			return false;
		}
	}

	/** Index of the first node */
	MBuint mFirst;
	/** Index of the second node */
	MBuint mSecond;
};

/**
  *	An MBCameraViewWithShiftVectorPCompare object is used for the comparison of
  * two MBCameraViewWithShiftVector objects.
  */
class MBCameraViewWithShiftVectorPCompare
{
public:
	/** Two MBCameraViewWithShiftVector are equal, when their image path is equal and their shift vector elements differ in TOLERANCE at the maximum */
	static double TOLERANCE;

	/**
	  *	Compares two MBCameraViewWithShiftVector objects and returns true if
	  * the first is "smaller" than the other one, false otherwise. At first
	  * their image path is compared, than the shift vector elements are
	  * compared. To be equal their image path must be equal and their shift
	  * vector elements differ in TOLERANCE at most.
	  *
	  * @param first const pointer to the first MBCameraViewWithShiftVector object in the comparison
	  * @param second const pointer to the second MBCameraViewWithShiftVector object in the comparison
	  * @return true if the first MBCameraViewWithShiftVector object is "smaller" than the other one, false otherwise
	  */
	bool operator()( const MBCameraViewWithShiftVector *first, const MBCameraViewWithShiftVector *second ) const
	{
		if (first->GetUndistortedImagePath() < second->GetUndistortedImagePath())
		{
			return true;
		}
		else if (first->GetUndistortedImagePath() > second->GetUndistortedImagePath())
		{
			return false;
		}
		else
		{
			double dif[2] = { first->GetTexcoordShiftVector()[U] - second->GetTexcoordShiftVector()[U],
				first->GetTexcoordShiftVector()[V] - second->GetTexcoordShiftVector()[V] };

			// const double TOLERANCE = 1.0E-6;
			
			if (dif[U] < -TOLERANCE)
			{
				return true;
			}
			else if (dif[U] > TOLERANCE)
			{
				return false;
			}
			else
			{
				if (dif[V] < -TOLERANCE)
				{
					return true;
				}

				return false;
			}
		}
	}
};

double MBCameraViewWithShiftVectorPCompare::TOLERANCE = 1.0E-6;

// Local struct and class definitions END
///////////////////////////////////////////////////////////////////////////////

/**
  *	Constructs a new MBEnergyMinimizer object with the given parameters
  *
  * @param nodes pointer to the array of MBTriangle objects, that have to be labeled
  * @param numNodes number of triangles in the array
  * @param labels pointer to the array MBCameraView objects, which act as labels for the MBTriangle objects
  * @param numLabels number of MBCameraView objects
  * @param modelBoundingBox axis aligned bounding box of the whole model that is labeled
  * @param useOnlyCamerasInModelBoundingBox true when only the cameras inside the axis aligned bounding box of the model should be considered in the labeling process, false otherwise
  */
MBEnergyMinimizer::MBEnergyMinimizer(
	const MBTriangle *const *nodes,
	const MBuint &numNodes,
	const MBCameraView *const *labels,
	const MBuint &numLabels,
	const MBAxisAlignedBoundingBox &modelBoundingBox,
	const bool &useOnlyCamerasInModelBoundingBox )
{
	MBuint index;

	// TEST BEGIN

	cout << "Model dimensions:" << endl;
	cout << "\tMin: " << modelBoundingBox.GetMin()[0] << " " << modelBoundingBox.GetMin()[1] << " " << modelBoundingBox.GetMin()[2] << endl;
	cout << "\tMax: " << modelBoundingBox.GetMax()[0] << " " << modelBoundingBox.GetMax()[1] << " " << modelBoundingBox.GetMax()[2] << endl;

	// TEST END

	mNodes.reserve( numNodes );
	mLabels.reserve( numLabels );

	for (index = 0; index < numNodes; index++)
	{
		mNodes.push_back( nodes[index] );
	}

	cout << "Sorting out unnecessary labels..." << endl;
	for (index = 0; index < numLabels; index++)
	{
		if (modelBoundingBox.IsIntersectingWithCameraViewFrustum( labels[index]->GetViewFrustum() ))
		{
			if (useOnlyCamerasInModelBoundingBox)
			{
				if (modelBoundingBox.IsPointInside( labels[index]->GetPosition() ))
				{
					mLabels.push_back( labels[index] );
				}
			}
			else
			{
				mLabels.push_back( labels[index] );
			}
		}
	}


	cout << "-\tNumber of labels after sorting out: " << mLabels.size() << " / " << numLabels << endl;
 
	mLabels.shrink_to_fit();

	mFilteringMode = MB_FILTERMODE_NEAREST_NEIGHBOR;
	mCameraViewsWithShiftVector = 0;
}

/**
  *	Destroys the MBEnergyMinimizer object and all the generated labels for
  * the MRFSV approach
  */
MBEnergyMinimizer::~MBEnergyMinimizer( void )
{
	if (mLabelingFData.size() > 0)
	{
		for (vector<MBCameraView *>::iterator it = mLabelingFData.begin();
			it != mLabelingFData.end(); it++)
		{
			delete *it;
		}
	}
}

/**
  *	Solves energy minimization problem and calculates thereby a valid mapping
  * of MBCameraView objects onto the MBTriangle objects
  *
  * @param options options for the labeling process (method, number of iterations etc.)
  * @return vector containing the calculated labeling, so that at vector[i] there is the label for the node[i]
  */
const std::vector<const MBCameraView*> &MBEnergyMinimizer::Solve(
	const MBLabelingOptions &options,
	MBDistanceMap *distanceMap ) throw( MBInvalidArgumentException )
{

	//mUseOctree = useOctree;
	m_distanceMap = distanceMap;
	mLabelingOptions = &options;
	mLabelingF.resize( mNodes.size() );

	// Delete old data
	if (mLabelingFData.size() > 0)
	{
		for (vector<MBCameraView *>::iterator it = mLabelingFData.begin();
			it != mLabelingFData.end(); it++)
		{
			delete *it;
		}

		mLabelingFData.clear();
	}

	const int NUM_NODES = static_cast<int>( mNodes.size() );
	const int NUM_LABELS = static_cast<int>( mLabels.size() );

	if (NUM_NODES == 0)
	{
		cout << "ERROR: No Nodes for labeling!" << endl;
		mLabelingF.clear();
		return mLabelingF;
	}

	if (NUM_LABELS == 0)
	{
		cout << "ERROR: No labels for labeling!" << endl;
		mLabelingF.clear();
		return mLabelingF;
	}

	// Print Labeling Options

	cout << "Chosen labeling options:" << endl;
	cout << "-\tChosen Labeling method: ";

	switch (options.GetLabelingMethod())
	{
	case MB_LABELING_MRF:
		cout << "Markov Random Field" << endl;
		break;
	case MB_LABELING_MRF_WITH_SHIFTING_VECTORS:
		cout << "Markov Random Field with Shift Vectors" << endl;
		break;
	default:
		cout << "Best Fragment" << endl;
		break;
	}

	cout << "-\tSight angle weight: " << options.GetAngleWeight() << endl;
	cout << "-\tDistance weight: " << 1.0 - options.GetAngleWeight() << endl;
	cout << "-\tMax. Distance: " << options.GetMaximumDistance() << endl;

	switch (options.GetLabelingMethod())
	{
		case MB_LABELING_MRF:
		case MB_LABELING_MRF_WITH_SHIFTING_VECTORS:
		{
			const MBLabelingOptionsGcoUsingMethod *mrf_options = static_cast<const MBLabelingOptionsGcoUsingMethod *>( &options );
			
			// Print labeling options for MRF and MRFSV

			cout << "-\tSmooth Cost Weight: " << mrf_options->GetSmoothCostWeight() << endl;
			cout << "-\tSingle Label Cost: " << mrf_options->GetLabelCost() << endl;
			cout << "-\tNumber of Iterations: " << mrf_options->GetNumIterations() << endl;

			mFilteringMode = mrf_options->GetFilteringMode();
			mVTermWeight = mrf_options->GetSmoothCostWeight();

			int *result = new int[NUM_NODES];   // stores result of optimization

			map<const MBTriangle *, int> node_to_index_map;
			// Fill the node_to_index_map
			for (int index = 0; index < NUM_NODES; index++)
			{
				node_to_index_map[mNodes[index]] = index;
			}

			MBuint current_index = 0;
			MBuint neighbor_index = 0;

			set<MBGraphCutNeighboringNodeIndices> node_pairs;

			MBuint index = 0;

			cout << "Finding triangle neighborhoods..." << endl;
			for (vector<const MBTriangle *>::const_iterator node_it = mNodes.begin();
				node_it != mNodes.end(); node_it++)
			{
				current_index = index++;

				// For every neighbor of the node
				for (set<MBTriangle *>::const_iterator neighbor_it = (*node_it)->GetNeighbors().begin();
					neighbor_it != (*node_it)->GetNeighbors().end(); neighbor_it++)
				{
					neighbor_index = node_to_index_map[(*neighbor_it)];

					node_pairs.insert( MBGraphCutNeighboringNodeIndices( current_index, neighbor_index ) );
				}
			}

			cout << "-\t" << node_pairs.size() << " neighborhoods found." << endl;

			// Deprecated
// 			if (useOctree)
// 			{
// 				cout << "-\tFinding Triangle intersections...." << endl;
// 				MBTriangle::indicesInIntersectingSightTriangles.clear();
// 				for (vector<const MBTriangle*>::const_iterator node_it = mNodes.begin();
// 					node_it != mNodes.end(); node_it++)
// 				{
// 					for (vector<const MBCameraView*>::const_iterator label_it = mLabels.begin();
// 						label_it != mLabels.end(); label_it++)
// 					{
// 						(*node_it)->AddIntersectingSightTriangleIndicesToSet( *label_it, options );
// 					}
// 				}
// 				cout << "-\tReady! Set has " << MBTriangle::indicesInIntersectingSightTriangles.size() << " entries." << endl;
// 			}
			
			if (options.GetLabelingMethod() == MB_LABELING_MRF_WITH_SHIFTING_VECTORS)
			{
				const MBLabelingOptionsMRFSV *mrfsv_options = static_cast<const MBLabelingOptionsMRFSV *>( &options );

				// Print labeling options for MRFSV
				cout << "-\tNum Levels for MRFSV: " << mrfsv_options->GetNumPyramidLevels() << endl;
				cout << "-\tNum Topmost labels to consider: " << mrfsv_options->GetNumTopmostLabelsToConsider() << endl;

				cout << "Calculating best Labeling..." << endl;

				const MBuint NUM_POSSIBLE_TRANSLATION_VECTORS = 9u;
				const MBuint NUM_PYRAMID_LEVELS = mrfsv_options->GetNumPyramidLevels();
				const MBuint NUM_TOPMOST_ENTRIES_TO_CONSIDER = mrfsv_options->GetNumTopmostLabelsToConsider();
				
				vector<MBCameraViewWithShiftVector*> all_labels;
				all_labels.resize( NUM_LABELS * NUM_POSSIBLE_TRANSLATION_VECTORS );

				map<int, set<int>> node_index_to_neighbor_indices_map;
				set<int> *current_set = 0;

				for (int node_index = 0; node_index < NUM_NODES; node_index++)
				{
					current_set = &node_index_to_neighbor_indices_map.insert( make_pair( node_index, set<int>() ) ).first->second;

					for (set<MBTriangle *>::const_iterator neighbor_it = mNodes[node_index]->GetNeighbors().begin();
						neighbor_it != mNodes[node_index]->GetNeighbors().end(); neighbor_it++)
					{
						current_set->insert( node_to_index_map[*neighbor_it] );
					}
				}

				node_to_index_map.clear();

				// 0 1 2
				// 3 4 5
				// 6 7 8

				// v
				// ^
				// |
				// |____> u


				const MBVec2 POSSIBLE_TRANSLATION_VECTORS[NUM_POSSIBLE_TRANSLATION_VECTORS] =
				{	MBVec2( -1.0, 1.0 ), MBVec2( 0.0, 1.0 ), MBVec2( 1.0, 1.0 ),
					MBVec2( -1.0, 0.0 ),  MBVec2( 0.0, 0.0 ), MBVec2( 1.0, 0.0 ),
					MBVec2( -1.0, -1.0), MBVec2( 0.0, -1.0 ), MBVec2( 1.0, -1.0 ) };

				MBVec2 *offsets = new MBVec2[NUM_LABELS * NUM_PYRAMID_LEVELS];

				MBuint image_width = 0u;
				MBuint image_height = 0u;

				vector<MBuint> num_samples_for_pyramid_levels( NUM_PYRAMID_LEVELS, 0 );

				MBuint max_side_length;
				double max_mean_length;
				double mean_length;
				double width_u;
				double height_v;

				vector<MBTextureCoordinate> texcoords;
				const double TEXCOORD_MIN = 0.0;
				const double TEXCOORD_MAX = 1.0;

				MBuint num_valid_nodes;

				cout << "-\tCalculating texcoord offsets..." << endl;

				max_mean_length = 0.0;

				// Calculate the offsets for the single layers
				for (MBuint label_index = 0; label_index < NUM_LABELS; label_index++)
				{
					mean_length = 0.0;
					num_valid_nodes = 0u;

					image_width = mLabels[label_index]->GetCameraModel()->GetIntrinsicParameters().mImageWidthInPixels;
					image_height = mLabels[label_index]->GetCameraModel()->GetIntrinsicParameters().mImageHeightInPixels;

					for (MBuint node_index = 0; node_index < NUM_NODES; node_index++)
					{
						if (!mNodes[node_index]->IsTriangleCompletelyInside( *mLabels[label_index], distanceMap ))
							continue;

						texcoords.clear();
						mNodes[node_index]->GetTextureCoords( mLabels[label_index], &texcoords );

						// Test between P0 and P1
						width_u = abs( texcoords[0].mU - texcoords[1].mU );
						height_v = abs( texcoords[0].mV - texcoords[1].mV );

						mean_length += max( width_u * image_width, height_v * image_height );

						// Test between P0 and P2
						width_u = abs( texcoords[0].mU - texcoords[2].mU );
						height_v = abs( texcoords[0].mV - texcoords[2].mV );

						mean_length += max( width_u * image_width, height_v * image_height );

						// Test between P1 and P2
						width_u = abs( texcoords[1].mU - texcoords[2].mU );
						height_v = abs( texcoords[1].mV - texcoords[2].mV );

						mean_length += max( width_u * image_width, height_v * image_height );
						
						num_valid_nodes++;
					}

					mean_length /= (3.0 * num_valid_nodes);

					//cout << "mean_length: " << mean_length << endl;

					max_mean_length = max( max_mean_length, mean_length );
					
					max_side_length = static_cast<MBuint>( ceil( max_mean_length ) );

					for (MBuint level_index = 0; level_index < NUM_PYRAMID_LEVELS; level_index++)
					{
						if (image_width == 0 || image_height == 0)
						{
							//cout << "ERROR: Too many pyramid levels for image " << GetFileNameFromPath( mLabels[label_index]->GetImagePath() ) << endl;
							delete [] result;
							delete [] offsets;

							throw MBInvalidArgumentException( "Too many pyramid levels for image " + GetFileNameFromPath( mLabels[label_index]->GetUndistortedImagePath() ) );
							//return mLabelingF;
						}

						//num_samples_for_pyramid_levels

						if (num_samples_for_pyramid_levels[level_index] < max_side_length)
							num_samples_for_pyramid_levels[level_index] = max_side_length;

						offsets[label_index * NUM_PYRAMID_LEVELS + level_index][U] = 1.0 / image_width;
						offsets[label_index * NUM_PYRAMID_LEVELS + level_index][V] = 1.0 / image_height;

						image_width /= 2u;
						image_height /= 2u;

						max_side_length = max( MBuint(1u), max_side_length / 2u );
					}
				}

				cout << "-\tNum Samples for levels:" << endl;
				MBuint samples_index = 0u;
				for (vector<MBuint>::const_iterator it = num_samples_for_pyramid_levels.begin();
					it != num_samples_for_pyramid_levels.end(); it++)
				{
					cout << "-\t\t" << samples_index++ << ": "<< *it << endl;
				}

				MBuint current_pyramid_level = NUM_PYRAMID_LEVELS - 1u;

				double trans_u = 0.0;
				double trans_v = 0.0;

				map<string, MBuint> image_path_to_label_index_map;

				// Start with the coarsest version
				for (MBuint label_index = 0; label_index < NUM_LABELS; label_index++)
				{
					image_path_to_label_index_map[mLabels[label_index]->GetUndistortedImagePath()] = label_index;

					for (MBuint transvector_index = 0;
						transvector_index < NUM_POSSIBLE_TRANSLATION_VECTORS;
						transvector_index++)
					{
						trans_u = POSSIBLE_TRANSLATION_VECTORS[transvector_index][U] * offsets[label_index * NUM_PYRAMID_LEVELS + current_pyramid_level][U];
						trans_v = POSSIBLE_TRANSLATION_VECTORS[transvector_index][V] * offsets[label_index * NUM_PYRAMID_LEVELS + current_pyramid_level][V];

						all_labels[label_index * NUM_POSSIBLE_TRANSLATION_VECTORS + transvector_index] =
							new MBCameraViewWithShiftVector( *mLabels[label_index], MBVec2( trans_u, trans_v) );
					}
				}

				// GCO V3.0
				mCameraViewsWithShiftVector = &all_labels;

				map<int, vector<int>> node_index_to_label_index_rank_map;
				vector<int> *current_label_indices_vector = 0;

				set<int> *current_neighbor_indices = 0;
				double current_weight = 0.0;

				multimap<double, int> weight_to_label_index_map;

				set<int> label_indices_for_next_iteration_set;
				MBuint index;

				set<MBCameraViewWithShiftVector*, MBCameraViewWithShiftVectorPCompare> labels_for_next_iteration;
				pair<set<MBCameraViewWithShiftVector*, MBCameraViewWithShiftVectorPCompare>::iterator, bool> labels_for_next_iteration_insert_pair;

				const MBVec2 *current_shift_vector = 0;
				MBuint original_label_index = 0;

				double current_d_weight = 0.0;
				double current_v_weight = 0.0;

				map<MBNodeIndicesWithLabelIndices, double>::const_iterator weight_v_indices_to_weight_map_it;

				MBCameraViewWithShiftVector *new_label;

				MBuint num_iter = 1u;
				// Iteration with iteration == -1 is for the half pixel shifting
				for (MBint iteration = current_pyramid_level; iteration >= -1; iteration--)
				{
					cout << "Iteration Nr. " << num_iter++ << " / " << NUM_PYRAMID_LEVELS + 1 << endl;
					GCoptimizationGeneralGraph generalGraph( NUM_NODES, static_cast<int>( all_labels.size() ) );
					generalGraph.setDataCost( MBEnergyMinimizer::DataCostFunctionWithShifting, this );
					generalGraph.setSmoothCost( MBEnergyMinimizer::SmoothCostFunctionWithShifting, this );
					generalGraph.setLabelCost( mrf_options->GetLabelCost() );

					for (set<MBGraphCutNeighboringNodeIndices>::const_iterator it = node_pairs.begin();
						it != node_pairs.end(); it++)
					{
						generalGraph.setNeighbors( static_cast<int>(it->mFirst), static_cast<int>(it->mSecond) );
					}
					
					if (iteration == -1)
					{
						// half pixel shifting
						mCurrentPyramidLevel = 0;
						MBImageManager::Get().SetExclusiveLevel( 0 );
						mNumSamples = num_samples_for_pyramid_levels[0];
					}
					else
					{
						mCurrentPyramidLevel = iteration;
						MBImageManager::Get().SetExclusiveLevel( iteration );
						mNumSamples = num_samples_for_pyramid_levels[iteration];
					}

					cout << "-\tDoing Graph-Cut Alpha Expansion..." << endl;
					double energy = generalGraph.expansion( static_cast<int>( mrf_options->GetNumIterations() ) );
					cout << "-\tReady! Energy after Alpha Expansion is " << energy << endl;

					mCurrentPyramidLevel = 0;
					MBImageManager::Get().SetExclusiveLevel( 0 );
					mNumSamples = num_samples_for_pyramid_levels[0];

					cout << "-\tCurrent Full Resolution Weight is " << generalGraph.compute_energy() << endl;

					for (int i = 0; i < NUM_NODES; i++)
					{
						result[i] = generalGraph.whatLabel( i );
					}

					if (iteration == -1)
					{
						// Ready!
						for (int index = 0; index < NUM_NODES; index++)
						{
							mLabelingF[index] = all_labels[result[index]];
							mLabelingFData.assign( all_labels.begin(), all_labels.end() );
						}
					}
					else
					{
						// There is at least one iteration to do
						cout << "Calculating the Labels for the next iteration..." << endl;
						if (NUM_TOPMOST_ENTRIES_TO_CONSIDER > 1u)
						{
							// Analyze the result
							node_index_to_label_index_rank_map.clear();

							for (int node_index = 0u; node_index < NUM_NODES; node_index++)
							{
								current_label_indices_vector = &node_index_to_label_index_rank_map.insert( make_pair( node_index, vector<int>() ) ).first->second;
								current_label_indices_vector->push_back( result[node_index] );

								weight_to_label_index_map.clear();

								// Calculate the V-Weights for the other labels, and sort the indices
								for (int label_index = 0; label_index < all_labels.size(); label_index++)
								{
									if (label_index != result[node_index])
									{
										// The best label does not need to be checked again

										current_weight = 0.0;
										current_neighbor_indices = &node_index_to_neighbor_indices_map[node_index];
										for (set<int>::const_iterator neighbor_it = current_neighbor_indices->begin();
											neighbor_it != current_neighbor_indices->end(); neighbor_it++)
										{
// 											weight_v_indices_to_weight_map_it =
// 												mWeightVIndicesToWeightMap.find(
// 												MBNodeIndicesWithLabelIndices(
// 												node_index, *neighbor_it,
// 												label_index, result[*neighbor_it] ) );
// 
// 											if (weight_v_indices_to_weight_map_it == mWeightVIndicesToWeightMap.end())
// 											{
// 												// Stop search here, invalid label for the node
// 												current_weight = -1.0;
// 												break;
// 											}
// 											else
// 												current_weight += weight_v_indices_to_weight_map_it->second;

											current_v_weight = mNodes[node_index]->GetWeightV( all_labels[label_index], mNodes[*neighbor_it], all_labels[result[*neighbor_it]], distanceMap, options, mFilteringMode, mCurrentPyramidLevel, mNumSamples );


											if (current_v_weight == MBTriangle::WEIGHT_V_INFINITY)
											{
												// Stop search here, invalid label for the node
												current_weight = -1.0;
												break;
											}
											else
											{
												current_weight += current_v_weight;
											}
										}

										if (current_weight != -1.0)
										{
											weight_to_label_index_map.insert( make_pair( current_weight, label_index ) );
										}
									}
								}

								for (multimap<double, int>::const_iterator weight_it = weight_to_label_index_map.begin();
									weight_it != weight_to_label_index_map.end(); weight_it++)
								{
									current_label_indices_vector->push_back( weight_it->second );
								}
							}

							label_indices_for_next_iteration_set.clear();

							for (map<int, vector<int>>::const_iterator node_it = node_index_to_label_index_rank_map.begin();
								node_it != node_index_to_label_index_rank_map.end(); node_it++)
							{
								for (index = 0u; index < NUM_TOPMOST_ENTRIES_TO_CONSIDER; index++)
								{
									if (index < node_it->second.size())
									{
										label_indices_for_next_iteration_set.insert( node_it->second[index] );
									}
								}
							}
						}
						else
						{
							label_indices_for_next_iteration_set.clear();

							for (MBuint index = 0; index < NUM_NODES; index++)
							{
								label_indices_for_next_iteration_set.insert( result[index] );
							}
						}

						cout << "Number of seed labels for the next iteration: " << label_indices_for_next_iteration_set.size() << " / max. " << all_labels.size() << endl;

						labels_for_next_iteration.clear();

						cout << "Constructing the Labels for the next iteration..." << endl;

						// Generate the labels for the next iteration
						for (set<int>::const_iterator label_index_it = label_indices_for_next_iteration_set.begin();
							label_index_it != label_indices_for_next_iteration_set.end();
							label_index_it++)
						{
							current_shift_vector = &all_labels[*label_index_it]->GetTexcoordShiftVector();
							original_label_index = image_path_to_label_index_map[all_labels[*label_index_it]->GetUndistortedImagePath()];

							MBCameraViewWithShiftVectorPCompare::TOLERANCE = min( offsets[original_label_index * NUM_PYRAMID_LEVELS + iteration][U] * 0.25, offsets[original_label_index * NUM_PYRAMID_LEVELS + iteration][V] * 0.25);

							for (index = 0u; index < NUM_POSSIBLE_TRANSLATION_VECTORS; index++)
							{
								if (iteration == 0)
								{
									// The next iteration is for the half pixel shifting
									trans_u = (*current_shift_vector)[U] + POSSIBLE_TRANSLATION_VECTORS[index][U] * offsets[original_label_index * NUM_PYRAMID_LEVELS + iteration][U] * 0.5;
									trans_v = (*current_shift_vector)[V] + POSSIBLE_TRANSLATION_VECTORS[index][V] * offsets[original_label_index * NUM_PYRAMID_LEVELS + iteration][V] * 0.5;
								}
								else
								{
									trans_u = (*current_shift_vector)[U] + POSSIBLE_TRANSLATION_VECTORS[index][U] * offsets[original_label_index * NUM_PYRAMID_LEVELS + (iteration - 1)][U];
									trans_v = (*current_shift_vector)[V] + POSSIBLE_TRANSLATION_VECTORS[index][V] * offsets[original_label_index * NUM_PYRAMID_LEVELS + (iteration - 1)][V];
								}

								new_label = new MBCameraViewWithShiftVector( *all_labels[*label_index_it], MBVec2( trans_u, trans_v ) );
								labels_for_next_iteration_insert_pair = labels_for_next_iteration.insert( new_label );

								if (!labels_for_next_iteration_insert_pair.second)
								{
									delete new_label;
									// Insertion was not successful
									//cout << "Rejected a label:\n\tOld one is MBVec2( " << (*labels_for_next_iteration_insert_pair.first)->GetTexcoordShiftVector()[U] << " " << (*labels_for_next_iteration_insert_pair.first)->GetTexcoordShiftVector()[V] << " )" << endl;
									//cout << "\tNew one is MBVec2( " << trans_u << " " << trans_v << " )" << endl; 
								}
							}
						}

						// Always add the "Non-Shifting" labels
// 						for (MBuint label_index = 0u; label_index < NUM_LABELS; label_index++)
// 						{
// 							new_label = new MBCameraViewWithShiftVector( *mLabels[label_index], MBVec2( 0.0, 0.0 ) );
// 							labels_for_next_iteration_insert_pair = labels_for_next_iteration.insert( new_label );
// 
// 							if (!labels_for_next_iteration_insert_pair.second)
// 							{
// 								delete new_label;
// 								// Insertion was not successful
// 								//cout << "Rejected a label:\n\tOld one is MBVec2( " << (*labels_for_next_iteration_insert_pair.first)->GetTexcoordShiftVector()[U] << " " << (*labels_for_next_iteration_insert_pair.first)->GetTexcoordShiftVector()[V] << " )" << endl;
// 								//cout << "\tNew one is MBVec2( " << trans_u << " " << trans_v << " )" << endl; 
// 							}
// 						}

						// Delete the labels from the last iteration
						for (vector<MBCameraViewWithShiftVector*>::iterator label_it = all_labels.begin();
							label_it != all_labels.end(); label_it++)
						{
							delete *label_it;
						}

						all_labels.assign( labels_for_next_iteration.begin(), labels_for_next_iteration.end() );

						cout << "Number of labels for the next iteration: " << all_labels.size() << endl;
					}
				}

				mCameraViewsWithShiftVector = 0;
				delete [] offsets;
			}
			else
			{
				MBuint max_edge_length_in_pixels = 0u;
				double mean_length;
				double max_mean_length;
				double width_u;
				double height_v;
				MBuint image_width;
				MBuint image_height;

				vector<MBTextureCoordinate> texcoords;
				const double TEXCOORD_MIN = 0.0;
				const double TEXCOORD_MAX = 1.0;

				const MBuint NUM_COMBINATIONS = NUM_LABELS * NUM_NODES;
				MBuint num_valid_nodes = 0u;

				max_mean_length = 0.0;
				mean_length = 0.0;

				cout << "Calculating best Labeling..." << endl;
				cout << "-\tCalculating number of samples per triangle..." << endl;

				// Calculate the number of samples
				for (MBuint label_index = 0; label_index < NUM_LABELS; label_index++)
				{
					mean_length = 0.0;
					num_valid_nodes = 0u;

					image_width = mLabels[label_index]->GetCameraModel()->GetIntrinsicParameters().mImageWidthInPixels;
					image_height = mLabels[label_index]->GetCameraModel()->GetIntrinsicParameters().mImageHeightInPixels;

					for (MBuint node_index = 0; node_index < NUM_NODES; node_index++)
					{
						if (!mNodes[node_index]->IsTriangleCompletelyInside( *mLabels[label_index], distanceMap ))
							continue;

						texcoords.clear();
						mNodes[node_index]->GetTextureCoords( mLabels[label_index], &texcoords );

						// Test between P0 and P1
						width_u = abs( texcoords[0].mU - texcoords[1].mU );
						height_v = abs( texcoords[0].mV - texcoords[1].mV );

						mean_length += max( width_u * image_width, height_v * image_height );

						// Test between P0 and P2
						width_u = abs( texcoords[0].mU - texcoords[2].mU );
						height_v = abs( texcoords[0].mV - texcoords[2].mV );

						mean_length += max( width_u * image_width, height_v * image_height );

						// Test between P1 and P2
						width_u = abs( texcoords[1].mU - texcoords[2].mU );
						height_v = abs( texcoords[1].mV - texcoords[2].mV );

						mean_length += max( width_u * image_width, height_v * image_height );

						num_valid_nodes++;
					}

					mean_length /= (3.0 * num_valid_nodes);

					max_mean_length = max( max_mean_length, mean_length );
				}

				mNumSamples = static_cast<MBuint>( ceil( max_mean_length ) );

				mCurrentPyramidLevel = 0u;
				MBImageManager::Get().SetExclusiveLevel( 0 );
				MBImageManager::Get().ResetNumberOfLoadedImages();
				MBImageManager::Get().ResetTimeNeededForLoading();
				MBTriangle::ms_AccumTimeForOcclusionDetectionSeconds = 0.0;

				cout << "-\tUsing " << mNumSamples << " samples per Triangle edge." << endl;
				cout << "-\tNum Nodes: " << NUM_NODES << ", Num Labels: " << NUM_LABELS << endl;

				node_to_index_map.clear();

				// GCO V3.0
				GCoptimizationGeneralGraph generalGraph( NUM_NODES, NUM_LABELS );
				generalGraph.setDataCost( MBEnergyMinimizer::DataCostFunction, this );
				generalGraph.setSmoothCost( MBEnergyMinimizer::SmoothCostFunction, this );
				generalGraph.setLabelCost( mrf_options->GetLabelCost() );

				for (set<MBGraphCutNeighboringNodeIndices>::const_iterator it = node_pairs.begin();
					it != node_pairs.end(); it++)
				{
					generalGraph.setNeighbors( static_cast<int>(it->mFirst), static_cast<int>(it->mSecond) );
				}

				node_pairs.clear();

				cout << "-\tDoing Graph-Cut Alpha Expansion..." << endl;

				MBTimer graph_expansion_timer;

				graph_expansion_timer.Toc();
				double energy = generalGraph.expansion( static_cast<int>( mrf_options->GetNumIterations() ) );
				double graph_expansion_time = graph_expansion_timer.Toc();

				//cout << "-\tReady! Weight after Alpha Expansion is " << energy << endl;
				cout << "-\tReady! Weight after Alpha Expansion is " << energy << ", Data Cost / Smooth Cost / Label Cost / lambda = " << generalGraph.giveDataEnergy() << " / " << generalGraph.giveSmoothEnergy() << " / " << generalGraph.giveLabelEnergy() << " / " << mVTermWeight << endl;
				cout << "-\tTimings:" << endl
					 << "-\tGraph Cut Alpha Expansion: " << graph_expansion_time << "s" << endl
					 << "-\t\tOcclusion Detection: " << MBTriangle::ms_AccumTimeForOcclusionDetectionSeconds << "s" << endl
					 << "-\t\tTime to load " << MBImageManager::Get().GetNumberOfLoadedImages() << " images: " << MBImageManager::Get().GetTimeInSecondsUsedForImageLoading() << "s" << endl;

				set<int> num_dif_labels;

				for (int i = 0; i < NUM_NODES; i++)
				{
					result[i] = generalGraph.whatLabel(i);

					num_dif_labels.insert( result[i] );
				}

				cout << "-\t Num different labels: " << num_dif_labels.size() << endl;

				for (int index = 0; index < NUM_NODES; index++)
				{
					mLabelingF[index] = mLabels[result[index]];
				}
			}

			delete [] result;

			break;
		}

		default: // for MB_LABELING_BEST_FRAGMENT and invalid values
		{
			double smallest_weight;
			double current_weight;

			cout << "Calculating best Labeling..." << endl;

			// For every node
			for (MBuint node_index = 0; node_index < NUM_NODES; node_index++)
			{
				smallest_weight = numeric_limits<double>::infinity();

				// Look for the label that produces the smallest weight
				for (MBuint label_index = 0; label_index < NUM_LABELS; label_index++)
				{
					current_weight = mNodes[node_index]->GetWeightD( mLabels[label_index], distanceMap, options );
					if (current_weight < smallest_weight)
					{
						smallest_weight = current_weight;
						mLabelingF[node_index] = mLabels[label_index];
					}
				}
			}
		}
	}

	return mLabelingF;
}

/**
  *	Returns the calculated data cost for the MRF approach
  *
  * @param nodeIndex index of the node
  * @param labelIndex index of the label
  * @param minimizer pointer to the MBEnergyMinimizer object
  * @return calculated data cost
  */
double MBEnergyMinimizer::DataCostFunction( int nodeIndex, int labelIndex, void *minimizer )
{
	MBEnergyMinimizer *my_minimizer = static_cast<MBEnergyMinimizer*>( minimizer );

	// This method seems to be very dumb
	//return my_minimizer->mNodes[nodeIndex]->GetWeightDIntegral( my_minimizer->mLabels[labelIndex], my_minimizer->mCurrentPyramidLevel );

	return my_minimizer->mNodes[nodeIndex]->GetWeightD( my_minimizer->mLabels[labelIndex], my_minimizer->m_distanceMap, *my_minimizer->mLabelingOptions );
}

/**
  *	Returns the calculated smooth cost for the MRF approach
  *
  * @param firstNodeIndex index of the first node
  * @param secondNodeIndex index of the second node
  * @param firstLabelIndex index of the first label
  * @param secondLabelIndex index of the second label
  * @param minimizer pointer to the MBEnergyMinimizer object
  * @return calculated smooth cost
  */
double MBEnergyMinimizer::SmoothCostFunction( int firstNodeIndex, int secondNodeIndex, int firstLabelIndex, int secondLabelIndex, void *minimizer )
{
	MBEnergyMinimizer *my_minimizer = static_cast<MBEnergyMinimizer*>( minimizer );

	return my_minimizer->mVTermWeight 
		
		*
		
		my_minimizer->mNodes[firstNodeIndex]->GetWeightV(
			my_minimizer->mLabels[firstLabelIndex],
			my_minimizer->mNodes[secondNodeIndex],
			my_minimizer->mLabels[secondLabelIndex],
			my_minimizer->m_distanceMap,
			*my_minimizer->mLabelingOptions,
			my_minimizer->mFilteringMode,
			my_minimizer->mCurrentPyramidLevel,
			my_minimizer->mNumSamples );
}

/**
  *	Returns the calculated data cost for the MRFSV approach
  *
  * @param nodeIndex index of the node
  * @param labelIndex index of the label
  * @param minimizer pointer to the MBEnergyMinimizer object
  * @return calculated data cost
  */
double MBEnergyMinimizer::DataCostFunctionWithShifting( int nodeIndex, int labelIndex, void *minimizer )
{
 	MBEnergyMinimizer *my_minimizer = static_cast<MBEnergyMinimizer*>( minimizer );

// 	map<MBNodeIndexWithLabelIndex, double>::const_iterator it = my_minimizer->mWeightDIndicesToWeightMap.find( MBNodeIndexWithLabelIndex( nodeIndex, labelIndex ) );
// 
// 	if (it == my_minimizer->mWeightDIndicesToWeightMap.end())
// 		return MBTriangle::WEIGHT_D_INFINITY;
// 
// 	return it->second;

 	return my_minimizer->mNodes[nodeIndex]->GetWeightD( (*my_minimizer->mCameraViewsWithShiftVector)[labelIndex], my_minimizer->m_distanceMap, *my_minimizer->mLabelingOptions );

	// This method seems to be very dumb
 	//return my_minimizer->mNodes[nodeIndex]->GetWeightDIntegral( (*my_minimizer->mCameraViewsWithShiftVector)[labelIndex], my_minimizer->mCurrentPyramidLevel );
}

/**
  *	Returns the calculated smooth cost for the MRFSV approach
  *
  * @param firstNodeIndex index of the first node
  * @param secondNodeIndex index of the second node
  * @param firstLabelIndex index of the first label
  * @param secondLabelIndex index of the second label
  * @param minimizer pointer to the MBEnergyMinimizer object
  * @return calculated smooth cost
  */
double MBEnergyMinimizer::SmoothCostFunctionWithShifting( int firstNodeIndex, int secondNodeIndex, int firstLabelIndex, int secondLabelIndex, void *minimizer )
{
	MBEnergyMinimizer *my_minimizer = static_cast<MBEnergyMinimizer*>( minimizer );
	
// 	map<MBNodeIndicesWithLabelIndices, double>::const_iterator it =
// 		my_minimizer->mWeightVIndicesToWeightMap.find(
// 		MBNodeIndicesWithLabelIndices(
// 		firstNodeIndex, secondNodeIndex,
// 		firstLabelIndex, secondLabelIndex ) );
// 
// 	if (it == my_minimizer->mWeightVIndicesToWeightMap.end())
// 		return MBTriangle::WEIGHT_V_INFINITY;
// 
// 	return it->second;

	return
		
		my_minimizer->mVTermWeight

		*

		my_minimizer->mNodes[firstNodeIndex]->GetWeightV(
			(*my_minimizer->mCameraViewsWithShiftVector)[firstLabelIndex],
			my_minimizer->mNodes[secondNodeIndex],
			(*my_minimizer->mCameraViewsWithShiftVector)[secondLabelIndex],
			my_minimizer->m_distanceMap,
			*my_minimizer->mLabelingOptions,
			my_minimizer->mFilteringMode,
			my_minimizer->mCurrentPyramidLevel,
			my_minimizer->mNumSamples );
}